/******************************************************************************
**
** File Name:	rtos_task.S
**
** Description:	Implementation of basic task management services.
**
** RTOS ver:	3.0
**
** Modules/Functions:
**
**			RTOS_TaskStart()
**			RTOS_TaskInit()
**			RTOS_TaskSuspend()
**			RTOS_TaskSuspendISR()
**			RTOS_TaskResume()
**			RTOS_TaskResumeISR()
**
** Notes:	none
** 		
*******************************************************************************
** History:		
**  
**	10/05/2011	- AK -	RTOS v2. TASK/ISR services reimplemented
**						kernel/task/isr typedefs are changed
**	10/15/2011	- AK -	ISR versions of RTOS_TaskResume() and RTOS_TaskSuspend()
**	10/15/2011	- AK -	RTOS_TaskGetId() moved to rtos_task_ex.S
**	11/20/2011  - AK - 	Removed obsolete RTOS_TaskSuspendISR() service
**	07/26/2012  - AK - 	RTOS API is downgraded to comply with earliest rtos v2
**  08/08/2012 	- AK - 	RTOS source code is checked for byte-by-byte complience 
**						with MPP binary (Dec 8, 2011).
**	09/20/2012	- AK - 	RTOS v3. _ENTER_CRITICAL/_LEAVE_CRITICAL 
**								 _RTOS_PROLOG/_RTOS_EPILOG
**	09/25/2012	- AK - 	RTOS_TaskResumeISR() / RTOS_TaskSuspendISR() added
**
******************************************************************************/
#include        "rtos.inc"

/******************************************************************************
;; MODULE::RTOS_TASK
******************************************************************************/
        _MODULE(rtos_task)
        _CODESECTION

/******************************************************************************
;; Prototype:	RTOS_PID_t * RTOS_TaskStart( RTOS_TCB_t * tcb )
;; Description:	Initialize the TaskID described in Task TCB.  Resume 
;;				Task at the end.
;; Passed:		R25:R24 = tcb
;; Returns:		R25:R24 = pid
;; Uses:		X, Z and R0
;; Calls:		RTOS_TaskInit(), RTOS_TaskResume()
;; Assumes:		
;; Notes:		None
******************************************************************************/
    _FUNCTION(RTOS_TaskStart)

RTOS_TaskStart:

    rcall   RTOS_TaskInit
    rjmp    RTOS_TaskResume
	
    _ENDFUNC	;; _FUNCTION(RTOS_TaskStart)
		
/******************************************************************************
;; Prototype:	RTOS_PID_t * RTOS_TaskInit( RTOS_TCB_t * tcb )
;; Description:	Initialize the TaskID described in TCF (task configuration 
;;				structure). Task is in Suspended state. To actually run the 
;;				task additional RTOS_TaskResume call is required.
;; Passed:		R25:R24 = &(tcb)
;; Returns:		R25:R24 = &(pid)
;; Uses:		X, Z and R0
;; Calls:		RTOS_TaskExit() if defined TASKEXIT
;; Assumes:		
;; Notes:		TaskControlBlock structure
;;				TaskID * - 2 bytes
;;				Flags	 - 1 byte
;;				Stack *	 - 2 bytes
;;				Entry *  - 2 bytes
******************************************************************************/
    _FUNCTION(RTOS_TaskInit)

RTOS_TaskInit:

    mov     Zl, p1l					;; Zl:Zh = addr(TCB)
    mov     Zh, p1h

	lpm		R0, Z+					;; Get Stack pointer
    mov     Xl, R0					;; Xl:Xh = addr(TCB)->stack
	lpm		R0, Z+
	mov     Xh, R0

#ifdef  TASKEXIT

    _EXTERN(RTOS_TaskExit)
	
    ldi     tmp2, pm_lo8(RTOS_TaskExit)
    st      X, tmp2      			;; "Store and Pre-dec" is not a PUSH
    ldi     tmp2, pm_hi8(RTOS_TaskExit)
    st      -X, tmp2      			;; Push task exit onto stack

	lpm		R0, Z+
    st      -X, R0
	lpm		R0, Z+
    st      -X, R0      			;; Push task entry point
	
#else

	lpm		R0, Z+					;; push Task' entry point to the stack
    st      X, R0
	lpm		R0, Z+
    st      -X, R0      			
	
#endif /* TASKEXIT */

    clr     tmp2
    ldi     tmp3, 33    			;; R0-R31 and SREG get "pushed"
	
PushRegisters:

    st      -X, tmp2				;; Fill rest of context with Zeros
    dec     tmp3        			
	
    brne    PushRegisters   		;; Decrement stack pointer

    sbiw    Xl, 1       			;; because pre dec is not a push!

	lpm     r1l, Z+					;; Get PID -> R23:22
	lpm		r1h, Z+
	lpm		R0, Z+					;; Priority -> R0
    mov     Zl, r1l
    mov     Zh, r1h

    std     Z+PidSP+NextL, Xl
    std     Z+PidSP+NextH, Xh     	;; addr(PID)->stack = Context frame pointer

    std     Z+PidPriority, R0		;; addr(PID)->prio = Task' priority
    ldi     Xl, BV(SuspendedBit) | BV(SuspendBit)
    std     Z+PidState, Xl  		;; addr(PID)->flags = Task' state (so we can "resume" it)
    std     Z+PidNext+NextH, tmp2	
    std     Z+PidNext+NextL, tmp2	;; addr(PID)->next = 0 clear queue link

    ret

    _ENDFUNC	;; _FUNCTION(RTOS_TaskInit)

/******************************************************************************
;; Prototype:	void RTOS_TaskSuspend(RTOS_PID_t * pid)
;; Description:	Mark TaskID as "suspend" and attempt to remove from
;;				the running task queue. If removed from the running task 
;;				queue, then mark as "suspended"
;; Passed:		p1h:p1l (R25:R24) = &(pid)
;; Returns:		None
;; Uses:		Y, Z, tmp0
;; Calls:		_EnterIsr, _RemoveObj, _LeaveISR
;; Assumes:		None
;; Notes:		None
******************************************************************************/
    _FUNCTION(RTOS_TaskSuspend)

RTOS_TaskSuspend:                    
	
	_ENTER_CRITICAL
    _RTOS_PROLOG

	_ENTER_CRITICAL
										;; Now we are on the System level
    mov     Zh, p1h
    mov     Zl, p1l
    ldd     tmp0, Z+PidState				
    sbr     tmp0, BV(SuspendBit)      	;; Mark process for suspending
    std     Z+PidState, tmp0

    mov     p2h, Zh
    mov     p2l, Zl
    ldi     Zh, hi8(RtosKernelData+RunQueue)
    ldi     Zl, lo8(RtosKernelData+RunQueue)
	
    rcall   _RemoveObj       			;; Attempt to remove from run queue
	
    mov     Yl, tmp0
    mov     Yh, tmp1
    adiw    Yl, 0
	
    breq    rt_st00

    ldd     Zl, Y+PidState
    sbr     Zl, BV(SuspendedBit)    	;; If found, then mark suspended.
    std     Y+PidState, Zl
	
rt_st00:

    _RTOS_EPILOG

    _ENDFUNC	;; _FUNCTION(RTOS_TaskSuspend)

/******************************************************************************
;; Prototype:	void RTOS_ISR_TaskSuspend(RTOS_PID_t *)
;; Description:	Mark TaskID as "suspend" and attempt to remove from
;;				the running task queue. If removed from the running task 
;;				queue, then mark as "suspended"
;; Passed:		p1h:p1l (R25:R24) = pointer to TaskID
;; Returns:		None
;; Uses:		Y, Z, tmp0
;; Calls:		_RemoveObj
;; Assumes:		Called from interrupt handlers
;; Notes:		None
;;*****************************************************************************
    _FUNCTION(RTOS_TaskSuspendISR)

RTOS_TaskSuspendISR:                    
	
	
    mov     Zh, p1h
    mov     Zl, p1l
	
	_ENTER_CRITICAL							;; Eneter critical (disable ALL interrupts)
	
    ldd     tmp0, Z+PidState				
    sbr     tmp0, BV(SuspendBit)      		;; Mark process for suspending
    std     Z+PidState, tmp0

    mov     p2h, Zh
    mov     p2l, Zl
    ldi     Zh, hi8(RtosKernelData+RunQueue)
    ldi     Zl, lo8(RtosKernelData+RunQueue)

    rcall   _RemoveObj       				;; Attempt to remove from run queue
	
    mov     Yl, tmp0
    mov     Yh, tmp1
    adiw    Yl, 0
	
    breq    SkipStateUpdate

    ldd     Zl, Y+PidState
    sbr     Zl, BV(SuspendedBit)    		;; If found, then mark suspended.
    std     Y+PidState, Zl
	
SkipStateUpdate:

	_LEAVE_CRITICAL							;; Leave critical (enable upper lavel interrupts)

	ret

    _ENDFUNC	;; _FUNCTION(RTOS_TaskSuspendISR)
	
/******************************************************************************
;; Prototype:	void RTOS_TaskResume(RTOS_PID_t *)
;; Description:	Insert specified TaskID into the running task queue. 
;;				Uses "suspend" and "suspended" flags to determine if task 
;;				is actually suspended or just marked for suspension. Will
;;				not actually resume task which is blocked (for example
;;				waiting for event), even it is marked for suspension.
;; Passed:		R25:24 = pointer to TaskID
;; Returns:		None
;; Uses:		Z, tmp0
;; Calls:		_EnqueueObj, _LeaveISR, _EnterISR
;; Assumes:		None
;; Notes:		None
******************************************************************************/
        _FUNCTION(RTOS_TaskResume)

RTOS_TaskResume:

	_ENTER_CRITICAL						;; Enter Critical section
	_RTOS_PROLOG						;; Enter system level and leave Critical section

    mov     Zl, p1l
    mov     Zh, p1h

	_ENTER_CRITICAL						;; Enter Critical again as the previous
										;; critical section was canceled by _EnterISR

    ldd     tmp0, Z+PidState
    cbr     tmp0, BV(SuspendBit)
    std     Z+PidState, tmp0  			;; clear Suspend flag

    sbrs    tmp0, SuspendedBit

    rjmp    rt_rt00

    cbr     tmp0, BV(SuspendedBit)  	;; If removed from run queue,
    std     Z+PidState, tmp0  			;; Clear flag and queue it up.

    rcall   _EnqueueObj       			;; If found, then add to run queue
	
rt_rt00:

    _RTOS_EPILOG						;; Leave system level and critical section
		

    _ENDFUNC	;; _FUNCTION(RTOS_TaskResume)

/******************************************************************************
;; Prototype:	void RTOS_TaskResumeISR(RTOS_PID_t * pid)
;; Description:	Insert specified TaskID into the running task queue. 
;;				Uses "suspend" and "suspended" flags to determine if task 
;;				is actually suspended or just marked for suspension. Will
;;				not actually resume task which is blocked (for example
;;				waiting for event), even it is marked for suspension.
;; Passed:		R25:24 = &(pid)
;; Returns:		None
;; Uses:		Z, tmp0
;; Calls:		_EnqueueObj
;; Assumes:		Called from interrupt handlers
;; Notes:		None
******************************************************************************/
	_FUNCTION(RTOS_TaskResumeISR)

RTOS_TaskResumeISR:

    mov     Zl, p1l						;; Zl:Zh = pid
    mov     Zh, p1h

	_ENTER_CRITICAL						;; Begin critical section (disable ALL interrupts)

    ldd     tmp0, Z+PidState			;; tmp0 = pid->flags 
    cbr     tmp0, BV(SuspendBit)		;; 
    std     Z+PidState, tmp0  			;; 
    sbrs    tmp0, SuspendedBit			;; 

    rjmp    SkipResuming				;; do nothing, just return

    cbr     tmp0, BV(SuspendedBit)  	;; If removed from run queue,
    std     Z+PidState, tmp0  			;; Clear flag and queue it up.

    rcall   _EnqueueObj       			;; If found, then add to run queue

SkipResuming:

	_LEAVE_CRITICAL						;; End critical section (enable upper lavel inetrupts)

	ret		

    _ENDFUNC	;; _FUNCTION(RTOS_TaskResumeISR)


    _END	;; _MODULE(rtos_task)

